/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file    stm32f4xx_it.c
  * @brief   Interrupt Service Routines.
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stm32f4xx_it.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#define Pi		3.14159265358979f

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN TD */
extern float angle_el;
float distant,angle_distant;
float angle_last;
float Uq_set=0;
float angle_i=0.01;
float iq_set=0;
float id_set=0;


extern float dang;
extern float vel_sp;
extern float angle_zero;
extern float vel_LPF;
extern float angle_speed,angle1_speed,angle1_old;
extern unsigned char mode,key;
extern float v_new;
extern float angle_sp;
extern float test;
extern ABC_Def i;
extern AlphaBeta_Def ab;
extern DQ_Def dq;
extern FOC_DEF foc;
float IIA,IIB;
extern unsigned char key;
/* USER CODE END TD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/* External variables --------------------------------------------------------*/
extern ADC_HandleTypeDef hadc1;
extern I2C_HandleTypeDef hi2c1;
extern TIM_HandleTypeDef htim1;
extern TIM_HandleTypeDef htim6;
extern TIM_HandleTypeDef htim7;
extern UART_HandleTypeDef huart1;
/* USER CODE BEGIN EV */

/* USER CODE END EV */

/******************************************************************************/
/*           Cortex-M4 Processor Interruption and Exception Handlers          */
/******************************************************************************/
/**
  * @brief This function handles Non maskable interrupt.
  */
void NMI_Handler(void)
{
  /* USER CODE BEGIN NonMaskableInt_IRQn 0 */

  /* USER CODE END NonMaskableInt_IRQn 0 */
  /* USER CODE BEGIN NonMaskableInt_IRQn 1 */
   while (1)
  {
  }
  /* USER CODE END NonMaskableInt_IRQn 1 */
}

/**
  * @brief This function handles Hard fault interrupt.
  */
void HardFault_Handler(void)
{
  /* USER CODE BEGIN HardFault_IRQn 0 */

  /* USER CODE END HardFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_HardFault_IRQn 0 */
    /* USER CODE END W1_HardFault_IRQn 0 */
  }
}

/**
  * @brief This function handles Memory management fault.
  */
void MemManage_Handler(void)
{
  /* USER CODE BEGIN MemoryManagement_IRQn 0 */

  /* USER CODE END MemoryManagement_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
    /* USER CODE END W1_MemoryManagement_IRQn 0 */
  }
}

/**
  * @brief This function handles Pre-fetch fault, memory access fault.
  */
void BusFault_Handler(void)
{
  /* USER CODE BEGIN BusFault_IRQn 0 */

  /* USER CODE END BusFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_BusFault_IRQn 0 */
    /* USER CODE END W1_BusFault_IRQn 0 */
  }
}

/**
  * @brief This function handles Undefined instruction or illegal state.
  */
void UsageFault_Handler(void)
{
  /* USER CODE BEGIN UsageFault_IRQn 0 */

  /* USER CODE END UsageFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_UsageFault_IRQn 0 */
    /* USER CODE END W1_UsageFault_IRQn 0 */
  }
}

/**
  * @brief This function handles System service call via SWI instruction.
  */
void SVC_Handler(void)
{
  /* USER CODE BEGIN SVCall_IRQn 0 */

  /* USER CODE END SVCall_IRQn 0 */
  /* USER CODE BEGIN SVCall_IRQn 1 */

  /* USER CODE END SVCall_IRQn 1 */
}

/**
  * @brief This function handles Debug monitor.
  */
void DebugMon_Handler(void)
{
  /* USER CODE BEGIN DebugMonitor_IRQn 0 */

  /* USER CODE END DebugMonitor_IRQn 0 */
  /* USER CODE BEGIN DebugMonitor_IRQn 1 */

  /* USER CODE END DebugMonitor_IRQn 1 */
}

/**
  * @brief This function handles Pendable request for system service.
  */
void PendSV_Handler(void)
{
  /* USER CODE BEGIN PendSV_IRQn 0 */

  /* USER CODE END PendSV_IRQn 0 */
  /* USER CODE BEGIN PendSV_IRQn 1 */

  /* USER CODE END PendSV_IRQn 1 */
}

/**
  * @brief This function handles System tick timer.
  */
void SysTick_Handler(void)
{
  /* USER CODE BEGIN SysTick_IRQn 0 */

  /* USER CODE END SysTick_IRQn 0 */
  HAL_IncTick();
  /* USER CODE BEGIN SysTick_IRQn 1 */

  /* USER CODE END SysTick_IRQn 1 */
}

/******************************************************************************/
/* STM32F4xx Peripheral Interrupt Handlers                                    */
/* Add here the Interrupt Handlers for the used peripherals.                  */
/* For the available peripheral interrupt handler names,                      */
/* please refer to the startup file (startup_stm32f4xx.s).                    */
/******************************************************************************/

/**
  * @brief This function handles ADC1, ADC2 and ADC3 global interrupts.
  */
void ADC_IRQHandler(void)
{
  /* USER CODE BEGIN ADC_IRQn 0 */

  /* USER CODE END ADC_IRQn 0 */
  HAL_ADC_IRQHandler(&hadc1);
  /* USER CODE BEGIN ADC_IRQn 1 */
//;
  /* USER CODE END ADC_IRQn 1 */
}

/**
  * @brief This function handles TIM1 break interrupt and TIM9 global interrupt.
  */
void TIM1_BRK_TIM9_IRQHandler(void)
{
  /* USER CODE BEGIN TIM1_BRK_TIM9_IRQn 0 */

  /* USER CODE END TIM1_BRK_TIM9_IRQn 0 */
  HAL_TIM_IRQHandler(&htim1);
  /* USER CODE BEGIN TIM1_BRK_TIM9_IRQn 1 */

  /* USER CODE END TIM1_BRK_TIM9_IRQn 1 */
}

/**
  * @brief This function handles TIM1 update interrupt and TIM10 global interrupt.
  */
void TIM1_UP_TIM10_IRQHandler(void)
{
  /* USER CODE BEGIN TIM1_UP_TIM10_IRQn 0 */

  /* USER CODE END TIM1_UP_TIM10_IRQn 0 */
  HAL_TIM_IRQHandler(&htim1);
  /* USER CODE BEGIN TIM1_UP_TIM10_IRQn 1 */

  /* USER CODE END TIM1_UP_TIM10_IRQn 1 */
}

/**
  * @brief This function handles TIM1 trigger and commutation interrupts and TIM11 global interrupt.
  */
void TIM1_TRG_COM_TIM11_IRQHandler(void)
{
  /* USER CODE BEGIN TIM1_TRG_COM_TIM11_IRQn 0 */

  /* USER CODE END TIM1_TRG_COM_TIM11_IRQn 0 */
  HAL_TIM_IRQHandler(&htim1);
  /* USER CODE BEGIN TIM1_TRG_COM_TIM11_IRQn 1 */

  /* USER CODE END TIM1_TRG_COM_TIM11_IRQn 1 */
}

/**
  * @brief This function handles I2C1 event interrupt.
  */
void I2C1_EV_IRQHandler(void)
{
  /* USER CODE BEGIN I2C1_EV_IRQn 0 */

  /* USER CODE END I2C1_EV_IRQn 0 */
  HAL_I2C_EV_IRQHandler(&hi2c1);
  /* USER CODE BEGIN I2C1_EV_IRQn 1 */

  /* USER CODE END I2C1_EV_IRQn 1 */
}

/**
  * @brief This function handles I2C1 error interrupt.
  */
void I2C1_ER_IRQHandler(void)
{
  /* USER CODE BEGIN I2C1_ER_IRQn 0 */

  /* USER CODE END I2C1_ER_IRQn 0 */
  HAL_I2C_ER_IRQHandler(&hi2c1);
  /* USER CODE BEGIN I2C1_ER_IRQn 1 */

  /* USER CODE END I2C1_ER_IRQn 1 */
}

/**
  * @brief This function handles USART1 global interrupt.
  */
void USART1_IRQHandler(void)
{
  /* USER CODE BEGIN USART1_IRQn 0 */

  /* USER CODE END USART1_IRQn 0 */
  HAL_UART_IRQHandler(&huart1);
  /* USER CODE BEGIN USART1_IRQn 1 */

  /* USER CODE END USART1_IRQn 1 */
}

/**
  * @brief This function handles TIM6 global interrupt, DAC1 and DAC2 underrun error interrupts.
  */
void TIM6_DAC_IRQHandler(void)
{
  /* USER CODE BEGIN TIM6_DAC_IRQn 0 */
		foc.the=i2c_AS5600_get_angle();
		foc.the_360=foc.the/(2*Pi)*360;
		foc.speed=get_speed();
		foc.speed=LPF_velocity(foc.speed); 
		float ADC_u,ADC_v,ADC_w;
		ADC_u=HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1);
		ADC_v=HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_2);
		ADC_w=HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_3);
	
		ADC_u=First_order_Filtering(ADC_u);
		ADC_v=First_order_Filtering(ADC_v);
	
		i.Ia=((ADC_u*3.3)/4096-1.65)*2;
		i.Ib=((ADC_v*3.3)/4096-1.65)*2;
		i.Ic=((ADC_w*3.3)/4096-1.65)*2;

		clarkeTransform(&i,&ab);
	
	angle_el=get_angle(angle_zero);
	
	
	parkTransform(&ab,angle_el,&dq);
//	dq.d=First_order_Filtering(dq.d);
//	dq.q=First_order_Filtering(dq.q);


//////------------------------------------------速度
		 if(mode==0)
	 {
		
	 	Uq_set = PID_velocity(vel_sp-foc.speed);
		 if(Uq_set<0)
		 {
			loop_motor(Uq_set,-angle_zero);
		 }
		 else
		 {
	 	loop_motor(Uq_set,angle_zero);
		 }
		 QT_output(vel_sp,foc.speed,Uq_set);
	 }	
////////---------------------------------------------�Ƕ�
//////
//////-----------------------------------------------角度
		if(mode==1)
		{
			QT_output(angle_sp,foc.the_360,Uq_set);
			if(foc.the_360-angle_last>180)
			{
				foc.the_360=foc.the_360-360;
			}
			if(foc.the_360-angle_last<-180)
			{
				foc.the_360=foc.the_360+360;
			}
			Uq_set=PID_angle(angle_sp-foc.the_360);
			if(Uq_set<0)
			{
			loop_motor(Uq_set,-angle_zero);
			}
			else
			{
			loop_motor(Uq_set,angle_zero);
			}
			angle_last=foc.the_360;
		}
		
//----------------------------------------阻尼
		if(mode==2)
		{
			test=angle_i*foc.speed;//0.01--0.04
		test=limit(test,-0.3,0.3);
		if(test>0)
		{
			loop_motor(test,angle_zero);	
		}
		if(test<0)
		{
			loop_motor(test,-angle_zero);	
		}
		QT_output(test,angle_i,0);
		}
//----------------------------------------------挡位		
		if(mode==3)
		{
			distant=360/dang*Pi/180;
			angle_distant=round(foc.the/distant)*distant;
			angle_sp=angle_distant/(2*Pi)*360;

			Uq_set=PID_angle(angle_sp-foc.the_360);
			
			if(Uq_set<0)
			{
			loop_motor(Uq_set,-angle_zero);
			}
			else
			{
			loop_motor(Uq_set,angle_zero);
			}
			angle_last=foc.the_360;
			QT_output(angle_sp,dang,foc.the_360);
		}
//-------------------------------------------------------
		if(mode==5)
		{
			iq_set=0.1;//PID_current_Q((0.1)-dq.q);
			id_set=0;//PID_current_D(0.1-dq.d);
			
			if(iq_set>=0)
			{
				angle_i=get_angle(angle_zero);
				svpwm(iq_set,id_set,angle_i);
			}
			if(iq_set<0)
			{
				angle_i=get_angle(-angle_zero);
				svpwm(iq_set,id_set,angle_i);
			}
			
		}
  /* USER CODE END TIM6_DAC_IRQn 0 */
  HAL_TIM_IRQHandler(&htim6);
  /* USER CODE BEGIN TIM6_DAC_IRQn 1 */

  /* USER CODE END TIM6_DAC_IRQn 1 */
}

/**
  * @brief This function handles TIM7 global interrupt.
  */
void TIM7_IRQHandler(void)
{
  /* USER CODE BEGIN TIM7_IRQn 0 */
//		static int before,now;
//		
//     now = key_get();
//	if(before==1&&now==0)
//	{
//		key=1;
//	}
//	else if(before==3&&now==0)
//	{
//		key=3;
//	}

//	before=now;

  HAL_TIM_IRQHandler(&htim7);
  /* USER CODE BEGIN TIM7_IRQn 1 */

  /* USER CODE END TIM7_IRQn 1 */
}

/* USER CODE BEGIN 1 */

/* USER CODE END 1 */
